package shortestpath.astar;

import java.util.Comparator;
import java.util.HashSet;
import java.util.List;
import java.util.PriorityQueue;
import java.util.Queue;
import java.util.Set;

import shortestpath.Graph;
import shortestpath.Node;
import shortestpath.ShortestPathFinder;
import shortestpath.astar.node.ListedNode;
import shortestpath.astar.node.NodeFactory;
import shortestpath.astar.node.OpenNode;
import shortestpath.astar.node.WeightedNode;

public class AStar<T extends Node> implements ShortestPathFinder<T> {

	private final Heuristics<T> heuristics;

	AStar(final Heuristics<T> heuristics) {
		this.heuristics = heuristics;
	}

	@Override
	public List<T> findShortestPath(Graph<T> g, T start, T destination) {
		final Queue<OpenNode<T>> openList = new PriorityQueue<OpenNode<T>>(16,
				new Comparator<OpenNode<T>>() {
					@Override
					public int compare(OpenNode<T> o1, OpenNode<T> o2) {
						return o1.getWeight() - o2.getWeight();
					}
				});
		openList.add(NodeFactory.convertToOpenNode(start, null, 0));
		final Set<T> closedList = new HashSet<T>();

		do {
			final ListedNode<T> current = openList.poll();
			if (current.asNode().equals(destination)) {
				return current.getWayFromStart();
			}
			this.expandNode(g, this.heuristics, openList, closedList, current,
					destination);
			closedList.add(current.asNode());
		} while (!openList.isEmpty());
		return null;
	}

	private void expandNode(final Graph<T> g, final Heuristics<T> h,
			final Queue<OpenNode<T>> openList, final Set<T> closedList,
			final ListedNode<T> current, final T destination) {
		s : for (T successor : g.getLinkedNodes(current.asNode())) {
			if (closedList.contains(successor)) {
				continue;
			}
			final int value = this.wayTo(g, current)
					+ g.getEdge(current.asNode(), successor).getWeight()
					+ h.estimate(successor, destination);

			for (WeightedNode wn : openList) {
				if (wn.equals(successor) && value >= wn.getWeight()) {
					continue s;
				}
			}

			final OpenNode<T> newOpenNode = NodeFactory.convertToOpenNode(
					successor, current, value);
			openList.remove(newOpenNode);
			openList.add(newOpenNode);
		}
	}

	private int wayTo(final Graph<T> g, final ListedNode<T> target) {
		ListedNode<T> current = target;
		int way = 0;
		while (current.getPredecessor() != null) {
			way += g.getEdge(current.getPredecessor().asNode(),
					current.asNode()).getWeight();
			current = current.getPredecessor();
		}
		return way;
	}
}
